// Device header
#include "App_Task.h"
#include <string.h> // 确保包含字符串处理函数

// MPU6050寄存器地址定义
#define MPU6050_ADDR 0xD0     // 假设AD0接地，地址为0xD0
#define MPU6050_WHO_AM_I 0x75 // WHO_AM_I寄存器地址

MPU6050_CalibrationData mpu_cal_data;
uint8_t calibration_done = 0;
/* 启动任务的配置 */
#define START_TASK_STACK 128
#define START_TASK_PRIORITY 1
TaskHandle_t start_task_handle;
void start_task(void *pvParameters);

TaskHandle_t test_task_handle;
void test_task(void *pvParameters);

/* 函数声明 */
uint8_t MPU6050_Read_ID(void); // 声明WHO_AM_I读取函数

/* Private typedef -----------------------------------------------------------*/
// 全局变量定义
int16_t ax, ay, az;        // 加速度数据
int16_t gx, gy, gz;        // 陀螺仪数据
int16_t temp;              // 温度数据
uint8_t uart_buf[256];     // UART发送缓冲区
uint8_t mpu_init_flag = 0; // MPU6050初始化标志

BaseType_t ret;
BaseType_t res;
// 读取MPU6050的WHO_AM_I寄存器
uint8_t MPU6050_Read_ID(void)
{
  uint8_t id;
  // 假设使用HAL库的I2C读取函数
  // 参数依次为：I2C句柄，设备地址，寄存器地址，地址长度，数据缓冲区，数据长度，超时时间
  HAL_I2C_Mem_Read(&hi2c2, MPU6050_ADDR, MPU6050_WHO_AM_I, 1, &id, 1, 100);
  return id;
}

void freertos_start(void)
{
  mpu_init_flag = MPU6050_Init();

  if (mpu_init_flag == 0)
  {
    HAL_UART_Transmit(&huart1, (uint8_t *)"MPU6050 initialized successfully\r\n", 31, 0xFFFF);
  }
  else
  {
    sprintf((char *)uart_buf, "MPU6050 init failed! Error code: %d\r\n", mpu_init_flag);
    HAL_UART_Transmit(&huart1, uart_buf, strlen((char *)uart_buf), 0xFFFF);
  }

  ret = xTaskCreate((TaskFunction_t)start_task,
                    (char *)"start_task",
                    (configSTACK_DEPTH_TYPE)START_TASK_STACK,
                    (void *)NULL,
                    (UBaseType_t)START_TASK_PRIORITY,
                    (TaskHandle_t *)&start_task_handle);

  if (ret == pdPASS)
  {
    HAL_UART_Transmit(&huart1, "start_task create success\r\n", 24, 0xFFFF);
  }

  res = xTaskCreate((TaskFunction_t)test_task,
                    (char *)"test_task",
                    (configSTACK_DEPTH_TYPE)128,
                    (void *)NULL,
                    (UBaseType_t)1,
                    (TaskHandle_t *)&test_task_handle);

  if (res == pdPASS)
  {
    HAL_UART_Transmit(&huart1, "test_task create success\r\n", 24, 0xFFFF);
  }

  vTaskStartScheduler();
}

void start_task(void *pvParameters)
{
  for (;;)
  {
    
    if (mpu_init_flag == 0)
    {
      HAL_UART_Transmit(&huart1, (uint8_t *)"exec 3\r\n", 8, 0xFFFF);
      uint8_t who_am_i = MPU6050_Read_ID();
      HAL_UART_Transmit(&huart1, (uint8_t *)"exec 6\r\n", 8, 0xFFFF);
      if (who_am_i != 0x68 && who_am_i != 0x69) // MPU6050的默认ID是0x68或0x69
      {
        HAL_UART_Transmit(&huart1, (uint8_t *)"exec 4\r\n", 8, 0xFFFF);
        sprintf((char *)uart_buf, "MPU6050 ID error: 0x%02X (expected 0x68 or 0x69)\r\n", who_am_i);
        HAL_UART_Transmit(&huart1, uart_buf, strlen((char *)uart_buf), 0xFFFF);
      }
      else
      {
        HAL_UART_Transmit(&huart1, (uint8_t *)"exec 5\r\n", 8, 0xFFFF);
        if (!calibration_done)
        {
          MPU6050_Calibrate(&mpu_cal_data, 30); // 采集1000个样本
          calibration_done = 1;
        }

        MPU6050_Read_Accel(&ax, &ay, &az);
        MPU6050_Read_Gyro(&gx, &gy, &gz);
        temp = MPU6050_Read_Temp();

        MPU6050_ApplyCalibration(&ax, &ay, &az, &gx, &gy, &gz, &mpu_cal_data);

        sprintf((char *)uart_buf,
                "\r\n-----------------------------\r\n"
                "Accel (raw):  ax:%6d, ay:%6d, az:%6d\r\n" // 加速度原始值
                "Gyro  (raw):  gx:%6d, gy:%6d, gz:%6d\r\n" // 陀螺仪原始值
                "Temperature:  %dC\r\n"                    // 温度
                "-----------------------------\r\n\r\n",
                ax, ay, az, gx, gy, gz, temp);
        HAL_UART_Transmit(&huart1, uart_buf, strlen((char *)uart_buf), 0xFFFF);
        HAL_UART_Transmit(&huart1, "\r\n sprintf\r\n", 24, 0xFFFF);
      }
    }
    else
    {
      HAL_UART_Transmit(&huart1, (uint8_t *)"MPU6050 not initialized\r\n", 25, 0xFFFF);
    }
    HAL_UART_Transmit(&huart1, (uint8_t *)"exec 2\r\n", 8, 0xFFFF);
    vTaskDelay(pdMS_TO_TICKS(200));

    HAL_UART_Transmit(&huart1, "1234", 4, 100);
    vTaskDelay(200);
  }
}

void test_task(void *pvParameters)
{
  for (;;)
  {
    HAL_UART_Transmit(&huart1, "123", 3, 100);
    vTaskDelay(500);
  }
}